/*
 * @Author: secrets123
 * @Date: 2022-07-21 10:53:28
 * @LastEditors: error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime: 2023-11-27 10:28:53
 * @FilePath: \stm32g431_AllRings\applications\epg_xx.c
 * @Description:
 *
 * Copyright (c) 2022 by secrets123 1097290967@qq.com, All Rights Reserved.
 */
#include <rtthread.h>
#include <rtconfig.h>
#include <board.h>
#include "rs485.h"
#include "epg_xx.h"
#include <agile_modbus.h>
// #include "allrings_sys.h"

#define DBG_TAG "rtu_master"
#define DBG_LVL DBG_ERROR // DBG_INFO
#include <rtdbg.h>

struct _epg_ctr epg_xx =
    {
        .epg_values.addr = 0X09,
        .epg_values.main_fsm_num = 0x00,

        /*epg_40*/
        .device_cmd_epg[_EPG40_R_STA_ENABLE] = EPG_XX_R_ENABLE,
        .device_cmd_epg[_EPG40_R_STA_WORKMODE] = EPG_XX_R_WORKMODE,
        .device_cmd_epg[_EPG40_R_STA_ACTION] = EPG_XX_R_ACTION,
        .device_cmd_epg[_EPG40_R_STA_GOLDCHECK] = EPG_XX_R_GOLDCHECK,
        .device_cmd_epg[_EPG40_R_STA_PREVALUES] = EPG_XX_R_PREVALUES,
        .device_cmd_epg[_EPG40_R_STA_3RINGS] = EPG_XX_R_3RING,
        .device_cmd_epg[_EPG40_R_STA_VOL] = EPG_XX_R_VOL,
        .device_cmd_epg[_EPG40_R_STA_TEMP] = EPG_XX_R_TEMP,

        .device_cmd_epg[_EPG40_W_ACTION_WITH_VALUES] = EPG_XX_W_CTREG_WORK_VALUES,
        .device_cmd_epg[_EPG40_W_ACTION_NO_VALUES] = EPG_XX_W_CTREG_WORK_NOVALUES,
        .device_cmd_epg[_EPG40_W_PRE_SET_VALUES] = EPG_XX_W_CTREG_PRESET_VALUE,

        /*指令合法性检查*/
        .inputcmd_valid_check = epg_cmd_check,
        /*CAN 数据反馈接口*/
        .can_sendback_data = epg_fdcan_send2up_data,
};

// epg_xx 空闲状态下，主任务
volatile uint16_t timer_cnt = 0;
void epg_xx_main_ctr(struct _epg_ctr *epg)
{
    static uint8_t enable_times = 0;

    if (++timer_cnt > 500)
    {
        timer_cnt = 0;
        if (++enable_times > 3) // 添加定时机制，增加动作间隔时间
        {
            enable_times = 0;
            // 当使能故障时，允许多次使能，减少使能失败的偶然性
            if (epg->epg_values.epg_enable_values.epg_enable_err_temp && epg->epg_values.epg_enable_values.epg_enable_auto_finished == 0)
            {
                if (++epg->epg_values.epg_enable_values.epg_enable_times > 5)
                {
                    epg->epg_values.epg_enable_values.epg_enable_times = 0;
                    epg->epg_values.epg_enable_values.epg_enable_auto_finished = 1; // 5次机会用完还是使能错误
                }
                else
                {
                    EPG_XX_W_CTREG_CTR(epg, 0); // 关闭异常使能
                    epg->epg_values.main_fsm_num = 1;
                }
            }
        }
    }
    else // 空闲时，周期查询EPG设备状态寄存器
    {
        if (timer_cnt % 80 == 0) // 50ms一次状态寄存器数据
            epg->epg_values.main_fsm_num = 2;
        if (timer_cnt % 300 == 0) // 500ms一次保持寄存器数据
            epg->epg_values.main_fsm_num = 3;
    }

    switch (epg->epg_values.main_fsm_num)
    {
    case 0:
        rt_thread_mdelay(3000);
        EPG_XX_R_INPUTREG_STA(epg);                     // 读状态寄存器数据
        if (epg->epg_values.epg_sta_reg_values.epg_err) // 初次上电的异常直接清除掉
        {
            epg->epg_values.epg_sta_reg_values.epg_err = 0;
        }
        epg->epg_values.main_fsm_num = 0x01; // 进入自动使能模式
        break;
    case 1:
        rt_thread_mdelay(100);
        EPG_XX_W_CTREG_CTR(epg, 1);                                // 使能EPG_XX
        epg->epg_values.epg_enable_values.epg_enable_err_temp = 0; // 清除使能故障
        epg->epg_values.main_fsm_num = 0xff;                       // 进入空闲模式
    case 2:
        EPG_XX_R_INPUTREG_STA(epg);          // 读状态寄存器数据
        epg->epg_values.main_fsm_num = 0xff; // 进入空闲模式
        break;
    case 3:
        EPG_XX_R_INPUTREG_KEEP(epg);         // 读保持寄存器数据
        epg->epg_values.main_fsm_num = 0xff; // 进入空闲模式
        break;
    default:
        //
        // if ((epg->epg_values.epg_enable_values.epg_enable_auto_finished = 0) &&
        //     (epg->epg_values.epg_enable_values.epg_enable_err_temp)) // 如果使能过程中存在故障
        // {
        //     EPG_XX_W_CTREG_CTR(epg, 1); // 使能EPG_XX
        //     if (++epg->epg_values.epg_enable_values.epg_enable_times > 5)
        //     {
        //         epg->epg_values.epg_enable_values.epg_enable_times = 0;         // 清零次数
        //         epg->epg_values.epg_enable_values.epg_enable_auto_finished = 1; // 使能完成
        //     }
        // }
        break;
    }
}

/*******************************************************************************/
// srt-注册指令检查
static uint8_t epg_cmd_check(uint8_t cmdcheck)
{
    uint8_t sta = 0;
    switch (cmdcheck)
    {

#define X_MACRO(a, b) case a:
        EPG_XX_CMD_LIST_TAB
#undef X_MACRO
        sta = 1;
        break;
    default:
        sta = 0;
        break;
    }
    return sta;
}

// CAN反馈到上层
static void epg_fdcan_send2up_data(struct _fdcan_rxdata *fdcandata)
{
    uint16_t id;
    uint8_t length;
    uint8_t *data;

    id = fdcandata->id;
    length = fdcandata->dta_length;
    data = fdcandata->data;

    // 获取到接收标志后,开启发送
    //  extern uint8_t all_can_rx_startflag;
    //  if (all_can_rx_startflag)
    FDCAN_Transmit(&hfdcan1, id, length, fdcandata->data);
}
// can-打包接口函数
static void epg_can_packed(struct _epg_ctr *epg, uint8_t *_fed_data, uint8_t _fed_length)
{
    memcpy(&epg->fdcan_fdback.data[1], _fed_data, _fed_length);
    // epg->fdcan_fdback.data[1]
    epg->fdcan_fdback.dta_length = _fed_length + 1;
    epg->can_sendback_data(&epg->fdcan_fdback);
}

// 解析状态寄存器数据
static void epg_xx_analysis_inputreg(struct _epg_ctr *epg, uint16_t *regdata)
{
    epg->epg_values.epg_sta_reg_values.epg_sta = (uint8_t)(regdata[0] & 0x00ff);     // 获取EPG状态
    epg->epg_values.epg_sta_reg_values.epg_err = (uint8_t)(regdata[1] & 0x00ff);     // 系统故障
    epg->epg_values.epg_sta_reg_values.epg_position = (uint8_t)(regdata[1] >> 8);    // 位置
    epg->epg_values.epg_sta_reg_values.epg_speed = (uint8_t)(regdata[2] & 0x00ff);   // 速度
    epg->epg_values.epg_sta_reg_values.epg_current = (uint8_t)(regdata[2] >> 8);     // 力矩
    epg->epg_values.epg_sta_reg_values.epg_voltage = (uint8_t)(regdata[3] & 0x00ff); // 电压
    epg->epg_values.epg_sta_reg_values.epg_temp = (uint8_t)(regdata[3] >> 8);        // 系统温度

    if (epg->epg_values.epg_sta_reg_values.epg_err) // 存在故障
    {
        uint8_t err = epg->epg_values.epg_sta_reg_values.epg_err;
        epg->epg_values.epg_enable_values.epg_enable_err_temp = err & 0x20;

        if ((epg->epg_values.epg_enable_values.epg_enable_err_temp) &&
            epg->epg_values.epg_enable_values.epg_enable_auto_finished == 0) // 存在使能故障
        {
            err &= ~0x20; // 清除上报的使能故障
        }
        rings_ctr.allrings_values.allsys_err.B.H |= err;
    }
    else
    {
        rings_ctr.allrings_values.allsys_err.B.H &= 0x00; // 原始故障清除掉
    }
}

// 解析保持寄存器数据
static void epg_xx_analysis_inputreg_keep(struct _epg_ctr *epg, uint16_t *regdata)
{
    epg->epg_values.epg_keep_reg_values.epg_ctr = (uint8_t)(regdata[0] & 0x00ff);
    epg->epg_values.epg_keep_reg_values.epg_nov_ctr = (uint8_t)(regdata[0] >> 8);
    epg->epg_values.epg_keep_reg_values.epg_s_p = (uint8_t)(regdata[1] >> 8);
    epg->epg_values.epg_keep_reg_values.epg_s_s = (uint8_t)(regdata[2] & 0x00ff);
    epg->epg_values.epg_keep_reg_values.epg_s_c = (uint8_t)(regdata[2] >> 8);

    // 装填8个预设参数
    for (uint8_t i = 0; i < 8; i++)
    {
        epg->epg_values.epg_keep_reg_values.epg_pre_values[i].epgv[0] = regdata[3 + i * 2];
        epg->epg_values.epg_keep_reg_values.epg_pre_values[i].epgv[1] = regdata[4 + i * 2];
    }
}

/*********************************************************************************************/
/********读状态寄存器数据-04*******/
uint8_t EPG_XX_R_INPUTREG_STA(struct _epg_ctr *epg)
{
    int rc;
    uint16_t Register[10];

    int send_len = agile_modbus_serialize_read_input_registers(ctx, EPG_XX_STA_ADDR, 4);
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        // srt_can_packed_u8(srt, DEVICE_RS485_ERR_TIMEOUT, NULL, 0);
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        rc = agile_modbus_deserialize_read_input_registers(ctx, read_len, Register);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 解析参数
            epg_xx_analysis_inputreg(epg, Register);
        }
    }
    return 0;
}

// 读保持寄存器参数
uint8_t EPG_XX_R_INPUTREG_KEEP(struct _epg_ctr *epg)
{
    int rc;
    uint16_t Register[20];

    int send_len = agile_modbus_serialize_read_input_registers(ctx, EPG_XX_CTR_ADDR, 12);
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        // srt_can_packed_u8(srt, DEVICE_RS485_ERR_TIMEOUT, NULL, 0);
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        rc = agile_modbus_deserialize_read_input_registers(ctx, read_len, Register);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 解析参数
            epg_xx_analysis_inputreg_keep(epg, Register);
        }
    }
    return 0;
}

// 读使能状态
uint8_t EPG_XX_R_ENABLE(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = epg->epg_values.epg_sta_reg_values.epg_sta & EPG_STA_BIT_ENABLE; // 读使能状态
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}
// 读工作模式
uint8_t EPG_XX_R_WORKMODE(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = (epg->epg_values.epg_sta_reg_values.epg_sta >> EPG_STA_BIT_MODE) & 0x01; // 工作模式
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}
// 读动作状态
uint8_t EPG_XX_R_ACTION(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = (epg->epg_values.epg_sta_reg_values.epg_sta >> EPG_STA_BIT_ACTION) & 0x01; // 动作状态
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}
// 读目标检测
uint8_t EPG_XX_R_GOLDCHECK(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = (epg->epg_values.epg_sta_reg_values.epg_sta >> EPG_STA_BIT_GOLDCHECK) & 0x03; // 目标检测
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}

// 读3环状态
uint8_t EPG_XX_R_3RING(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = epg->epg_values.epg_sta_reg_values.epg_position; // 装填位置信息
    epg->fdcan_fdback.data[2] = epg->epg_values.epg_sta_reg_values.epg_speed;    // 装填速度信息
    epg->fdcan_fdback.data[3] = epg->epg_values.epg_sta_reg_values.epg_current;  // 装填力信息
    epg->fdcan_fdback.dta_length = 4;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}
// 读温度
uint8_t EPG_XX_R_TEMP(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = epg->epg_values.epg_sta_reg_values.epg_temp; // 装填温度信息
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}
// 读电压
uint8_t EPG_XX_R_VOL(struct _epg_ctr *epg)
{

    epg->fdcan_fdback.data[1] = epg->epg_values.epg_sta_reg_values.epg_voltage; // 装填电压信息
    epg->fdcan_fdback.dta_length = 2;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}

// 读预设参数
uint8_t EPG_XX_R_PREVALUES(struct _epg_ctr *epg)
{
    uint8_t *src;
    uint8_t num = epg->fdcan_fdback.data[1];
    if (num < 8 || num > 15) // 限制参数查询范围
    {
        return 0;
    }

    src = (uint8_t *)&epg->epg_values.epg_keep_reg_values.epg_pre_values[num - 8].bytes;
    memcpy(&epg->fdcan_fdback.data[2], src, 3);
    epg->fdcan_fdback.dta_length = 5;
    epg->can_sendback_data(&epg->fdcan_fdback);
    return 0;
}

/*******************************控制状态寄存器**************************************/
// 使能/停止电动夹爪
uint8_t EPG_XX_W_CTREG_CTR(struct _epg_ctr *epg, uint8_t ctr_flag)
{
    uint8_t sta;
    uint16_t Register[2];

    if (ctr_flag)
        Register[0] = 0x0001; // 使能夹爪
    else
        Register[0] = 0; // 失能夹爪

    int send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_ADDR, 1, Register);
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        int rc = agile_modbus_deserialize_write_registers(ctx, read_len);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 反馈数据
            sta = 0x01; // 设置成功
            epg_can_packed(epg, &sta, 1);
        }
    }
    return 0;
}

// 夹爪有参数运行
uint8_t EPG_XX_W_CTREG_WORK_VALUES(struct _epg_ctr *epg)
{
    uint8_t sta;
    uint8_t pre_p, pre_s, pre_c;
    uint16_t Register[3];

    pre_p = epg->fdcan_fdback.data[1];
    pre_s = epg->fdcan_fdback.data[2];
    pre_c = epg->fdcan_fdback.data[3];

    Register[0] = 0x0009;
    Register[1] = (uint16_t)pre_p << 8 | 0x0000; // 位置参数
    Register[2] = (uint16_t)pre_c << 8 | pre_s;  // 速度+力矩

    int send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_ADDR, 3, Register);
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        int rc = agile_modbus_deserialize_write_registers(ctx, read_len);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 反馈数据
            sta = 0x01; // 设置成功
            epg_can_packed(epg, &sta, 1);
        }
    }
    return 0;
}

// 夹爪无参数运行
uint8_t EPG_XX_W_CTREG_WORK_NOVALUES(struct _epg_ctr *epg)
{
    uint8_t sta;
    uint8_t pre_num;
    uint16_t Register[3];

    pre_num = epg->fdcan_fdback.data[1];

    if (pre_num == 0 && pre_num > 15) // 查看参数是否合法
    {
        return 1;
    }
    Register[0] = (uint16_t)pre_num << 8 | 0x0B;

    int send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_ADDR, 1, Register);
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        int rc = agile_modbus_deserialize_write_registers(ctx, read_len);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 反馈数据
            sta = 0x01; // 设置成功
            epg_can_packed(epg, &sta, 1);
        }
    }
    return 0;
}

// 设置预设参数
uint8_t EPG_XX_W_CTREG_PRESET_VALUE(struct _epg_ctr *epg)
{
    uint8_t sta;
    uint8_t pre_num, pre_p, pre_s, pre_c;
    uint16_t Register[2];
    int send_len;

    pre_num = epg->fdcan_fdback.data[1];
    pre_p = epg->fdcan_fdback.data[2];
    pre_s = epg->fdcan_fdback.data[3];
    pre_c = epg->fdcan_fdback.data[4];

    if (pre_num < 8 && pre_num > 15) // 查看参数是否合法
    {
        return 1;
    }
    Register[0] = (uint16_t)pre_s << 8 | pre_p; // 按照位置-速度-力填充预设数据
    Register[1] = ((uint16_t)pre_c & 0x00ff);

    switch (pre_num)
    {
    case 0x08:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET1, 2, Register);
        break;
    case 0x09:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET2, 2, Register);
        break;
    case 0x0A:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET3, 2, Register);
        break;
    case 0x0B:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET4, 2, Register);
        break;
    case 0x0C:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET5, 2, Register);
        break;
    case 0x0D:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET6, 2, Register);
        break;
    case 0x0E:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET7, 2, Register);
        break;
    case 0x0F:
        send_len = agile_modbus_serialize_write_registers(ctx, EPG_XX_CTR_PRESET8, 2, Register);
        break;
    default:
        return 0;
        break;
    }
    rs485_send(ctx->send_buf, send_len);
    int read_len = rs485_receive(ctx->read_buf, ctx->read_bufsz, 100, 5);

    if (read_len == 0)
    {
        if (++epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter >= DEVICE_RS485_ERR_THRESHOLD)
        {
            epg->epg_values.epg_com_err_judge.rs485_err_timeout_counter = 0;
            rings_ctr.allrings_values.allsys_err.bit.RS485_TIMEOUT = 1;
        }
    }
    else
    {
        int rc = agile_modbus_deserialize_write_registers(ctx, read_len);
        if (rc < 0) // 如果反馈数据错误
        {
            if (++epg->epg_values.epg_com_err_judge.rs485_err_failure_counter >= DEVICE_RS485_ERR_THRESHOLD)
            {
                epg->epg_values.epg_com_err_judge.rs485_err_failure_counter = 0;
                rings_ctr.allrings_values.allsys_err.bit.RS485_ERR = 1;
            }
            LOG_W("Receive failed.");
            if (rc != -1)
                LOG_W("Error code:%d", -128 - rc);
        }
        else
        {
            // 清除系统485异常统计

            for (int i = 0; i < rc; i++)
                LOG_I("[---0x%02X---]Register [%d]: 0x%02X", 01, i, Register[i]);

            // 反馈数据
            sta = 0x01; // 设置成功
            epg_can_packed(epg, &sta, 1);
        }
    }
    return 0;
}
